/*
Copyright (c) [2019年5月1日] [吴超]
[MBT_Studio] is licensed under Mulan PSL v2.
You can use this software according to the terms and conditions of the Mulan PSL v2.
You may obtain a copy of Mulan PSL v2 at:
		 http://license.coscl.org.cn/MulanPSL2
THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY KIND,
EITHER EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO NON-INFRINGEMENT,
MERCHANTABILITY OR FIT FOR A PARTICULAR PURPOSE.
See the Mulan PSL v2 for more details.
*/
#pragma once
#include <assert.h>
#include <向量.h>
#include <公共函数.h>

#include <Vector/向量运算.h>

#include <A_CUDA数学Core/vec2.h>
#include <A_CUDA数学Core/vec3.h>
#include <A_CUDA数学Core/vec4.h>
#include <A_CUDA数学Core/point3.h>
#include <A_CUDA数学Core/maths.h>

#include "几何计算.h"


#include <cassert>


Inline vec4 f_graph_plane(const vec3& n, float32 d) {
	return {n.x, n.y, n.z, d};
}


Inline vec4 f_graph_quat相乘(const vec4& q1, const vec4& q2) {
	vec4 newPos;
	newPos.x = q1.w * q2.x + q2.w * q1.x + q1.y * q2.z - q2.y * q1.z;
	newPos.y = q1.w * q2.y + q2.w * q1.y + q1.z * q2.x - q2.z * q1.x;
	newPos.z = q1.w * q2.z + q2.w * q1.z + q1.x * q2.y - q2.x * q1.y;
	newPos.w = q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q2.z * q1.z;
	return newPos;
}


Inline vec4 f_graph_quat反向(const vec4& q) {
	return {-q.x, -q.y, -q.z, q.w};
}


CUDA_CALLABLE inline vec4 f_graph_欧拉角转四元数2(const vec3& axis) {
	vec3 xyz = axis;// *0.5f;
	//(xyz *= (xyz, float32(0.5)));
	xyz.x = M_角度转弧度(xyz.x);
	xyz.y = M_角度转弧度(xyz.y);
	xyz.z = M_角度转弧度(xyz.z);

	vec_mul(&xyz, 0.5);
	vec4 q;

	q.x = sin(xyz.y) * sin(xyz.z) * cos(xyz.x) + cos(xyz.y) * cos(xyz.z) * sin(xyz.x);
	q.y = sin(xyz.y) * cos(xyz.z) * cos(xyz.x) + cos(xyz.y) * sin(xyz.z) * sin(xyz.x);
	q.z = cos(xyz.y) * sin(xyz.z) * cos(xyz.x) - sin(xyz.y) * cos(xyz.z) * sin(xyz.x);
	q.w = cos(xyz.y) * cos(xyz.z) * cos(xyz.x) - sin(xyz.y) * sin(xyz.z) * sin(xyz.x);

	return q;
}



CUDA_CALLABLE inline vec4 f_graph_欧拉角转四元数(const vec3& axis) {
	vec3 xyz = axis;
	xyz.x = M_角度转弧度(xyz.x);
	xyz.y = M_角度转弧度(xyz.y);
	xyz.z = M_角度转弧度(xyz.z);

	//float32 TEST = q.w * q.y - q.x * q.z;
	vec_mul(&xyz, 0.5);
	vec4 q;

	float32 cy = cos(xyz.z);
	float32 sy = sin(xyz.z);
	float32 cp = cos(xyz.y);
	float32 sp = sin(xyz.y);
	float32 cr = cos(xyz.x);
	float32 sr = sin(xyz.x);

	q.w = cr * cp * cy + sr * sp * sy;
	q.x = sr * cp * cy - cr * sp * sy;
	q.y = cr * sp * cy + sr * cp * sy;
	q.z = cr * cp * sy - sr * sp * cy;
	
	return q;
}


CUDA_CALLABLE Inline vec3 f_graph_四元数转欧拉角(const vec4& q) {
	vec3 angles;

	/*double q0, q1, q2, q3;
	q0 = q.w;
	q1 = q.x;
	q2 = q.y;
	q3 = q.z;
	angles.y = (float)(asin(-2 * q1 * q3 + 2 * q0 * q2) * 57.3); // pitch
	angles.x = (float)(atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * 57.3); // roll
	angles.z = (float)(atan2(2 * (q1 * q2 + q0 * q3), q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * 57.3);

	//angles.x = M_弧度转角度(angles.x);
	//angles.y = M_弧度转角度(angles.y);
	//angles.z = M_弧度转角度(angles.z);

	return angles;*/
	/*float32 r11 = (-2 * (q.y * q.z - q.w * q.x));
	float32 r12 = q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z;
	float32 r21 = 2 * (q.x * q.z + q.w * q.y);
	float32 r31 = -2 * (q.x * q.y - q.w * q.z);
	float32 r32 = q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z;
		
	angles.x = atan2(r31, r32);
	angles.y = asin(r21);
	angles.z = atan2(r11, r12);

	angles.x = M_弧度转角度(angles.x);
	angles.y = M_弧度转角度(angles.y);
	angles.z = M_弧度转角度(angles.z);

	return angles;*/

	// roll (x-axis rotation)
	float64 sinr_cosp = 2 * (q.w * q.x + q.y * q.z);
	float64 cosr_cosp = 1 - 2 * (q.x * q.x + q.y * q.y);
	angles.x = atan2(sinr_cosp, cosr_cosp);
	
	// pitch (y-axis rotation)
	double sinp = 2 * (q.w * q.y - q.z * q.x);
	if (abs(sinp) >= 1)
		angles.y = copysign(M_PI / 2, sinp); // use 90 degrees if out of range
	else
		angles.y = asin(sinp);

	// yaw (z-axis rotation)
	float64 siny_cosp = 2 * (q.w * q.z + q.x * q.y);
	float64 cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z);
	angles.z = atan2(siny_cosp, cosy_cosp);

	angles.x = M_弧度转角度(angles.x);
	angles.y = M_弧度转角度(angles.y);
	angles.z = M_弧度转角度(angles.z);
	
	return angles;
}



inline Quat f_Quat_转换(const vec3& rhs) {
	return Quat(rhs.x, rhs.y, rhs.z, 0.0f);
}

inline Quat f_Quat_转换(const vec4& rhs) {
	return Quat(rhs.x, rhs.y, rhs.z, rhs.w);
}


CUDA_CALLABLE inline vec4 f_graph_quatNormalize(const vec4& q) {
	float32 lSq = q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w;

	if (lSq > 0.0f) {
		float32 invL = 1.0f / sqrtf(lSq);
		return q * invL;
	}
	else
		return {0,0,0,1};
}


Inline vec4 f_graph_从轴向角度计算四元数(const vec3& 归一化向量, float 弧度) {
	float32 half = 弧度 *0.5f;
	float32 w = cosf(half);

	const float32 sin_theta_over_two = sinf(half);
	vec3 v = 归一化向量 * sin_theta_over_two;

	return f_graph_quatNormalize({ v.x, v.y, v.z, w });
}

Inline vec4 f_graph_旋转角转四元数(const vec3& 旋转角) {
	vec4 x = f_graph_从轴向角度计算四元数({ 1,0,0 }, M_角度转弧度(旋转角.x));
	vec4 y = f_graph_从轴向角度计算四元数({ 0,0,1 }, M_角度转弧度(旋转角.y));
	vec4 z = f_graph_从轴向角度计算四元数({ 0,1,0 }, M_角度转弧度(旋转角.z));
	x = f_graph_quat相乘(x, z);
	x = f_graph_quat相乘(x, y);
	//x = f_graph_quat相乘(y, x);
	//x = f_graph_quat相乘(z, x);
	return x;
}


Inline vec3 f_graph_quat反向变换坐标(const vec4& q, const vec3& pos) {
	vec3 qv = { q.x, q.y, q.z };
	vec3 newPos = pos * (2.0f * q.w * q.w - 1.0f);
	newPos = newPos - (vec_cross(qv, pos) * q.w * 2.0f) + (qv * vec_dot(qv, pos) * 2.0f);
	return newPos;
}

Inline vec3 f_graph_quatTranform(const vec4& q, const vec3& pos) {
	vec3 qv = { q.x, q.y, q.z };
	vec3 newPos = pos * (2.0f * q.w * q.w - 1.0f);
	newPos = newPos + (vec_cross(qv, pos) * q.w * 2.0f) + (qv * vec_dot(qv, pos) * 2.0f);
	return newPos;
}

CUDA_CALLABLE Inline vec3 vec_绕轴旋转(const vec4& dir, const vec3 oldPos) {
	vec3 r;
	//float32 a = dir.w * M_PI / 180;
	float32 a = dir.w;
	float32 c = cos(a);
	float32 s = sin(a);

	r.x = (dir.x*dir.x * (1 - c) + c) * oldPos.x + (dir.x*dir.y * (1 - c) - dir.z * s) * oldPos.y + (dir.x * dir.z * (1 - c) + dir.y * s) * oldPos.z;
	r.y = (dir.y*dir.x * (1 - c) + dir.z * s) * oldPos.x + (dir.y * dir.y * (1 - c) + c) * oldPos.y + (dir.y * dir.z * (1 - c) - dir.x * s) * oldPos.z;
	r.z = (dir.x * dir.z * (1 - c) - dir.y * s) * oldPos.x + (dir.y * dir.z * (1 - c) + dir.x * s) * oldPos.y + (dir.z * dir.z * (1 - c) + c) * oldPos.z;

	return r;
}

/*CUDA_CALLABLE Inline vec3 f_quat_tranform(const vec4& m1, const vec3& m2) {
	vec3 r;
	r.x = m2.x * m1.x + m2.x * m1.y + m2.x * m1.z + m2.x * m1.w;
	r.y = m2.y * m1.x + m2.y * m1.y + m2.y * m1.z + m2.y * m1.w;
	r.z = m2.z * m1.x + m2.z * m1.y + m2.z * m1.z + m2.z * m1.w;
	return r;
}*/

/*Inline vec3 f_graph_方向转角度(const vec3& dir) {
	vec3 euler;
	euler.x = acosf(sqrtf((dir.x * dir.x + dir.z * dir.z) / (dir.x * dir.x + dir.y * dir.y + dir.z * dir.z)));
	euler.x = M_弧度转角度(euler.x);

	euler.y = M_弧度转角度(atan2f(dir.x, dir.z));

	if (euler.y < 0) euler.y += 180;

}*/



Inline vec4 f_graph_quat两向量(const vec3& 归一化_dirA, const vec3& 归一化_dirB) {
	float32 弧度AB = f_graph_两向量夹角弧度180(归一化_dirA, 归一化_dirB);
	return f_graph_从轴向角度计算四元数(vec_normalize2(vec_cross(归一化_dirA, 归一化_dirB)), 弧度AB);
}





#define IX(x, y, z) ((x) + (y) * N + (z) * N * N)
CUDA_CALLABLE Inline uint64 f_坐标转线性索引(const uvec3& coord, const uvec3& dim) {
	uint32 p = dim.x * dim.y;
	return uint64(coord.z) * p + uint64(coord.y) * dim.x + coord.x;
}

CUDA_CALLABLE Inline uvec3 f_线性索引转坐标(const uint64 index, const uvec3& dim) {
	uint32 p = dim.x * dim.y;
	uvec3 coord;
	coord.z = index / p;
	coord.y = (index - (coord.z * p)) / dim.x;
	coord.x = index - ((coord.z * p) + (coord.y * dim.x));
	return coord;
}

CUDA_CALLABLE Inline uint64 f_坐标转线性索引_i(const ivec3& coord, const uvec3& dim) {
	uint32 p = dim.x * dim.y;
	return uint64(coord.z) * p + uint64(coord.y) * dim.x + coord.x;
}

CUDA_CALLABLE Inline uvec3 f_浮点坐标转索引坐标(const vec3& coord, const vec3& offset, float32 间隔缩放 = 1.0) {
	uvec3 dim;
	dim.x = (coord.x - offset.x) / 间隔缩放;
	dim.y = (coord.y - offset.y) / 间隔缩放;
	dim.z = (coord.z - offset.z) / 间隔缩放;
	return dim;
}





CUDA_CALLABLE Inline vec3 f_矩阵相乘(const mat3X3& m1, const vec3& v2) {
	vec3 r = vec_mul(m1.row0, v2.x);
	vec_add(&r, vec_mul(m1.row1, v2.y));
	vec_add(&r, vec_mul(m1.row2, v2.z));
	return r;
}

CUDA_CALLABLE Inline mat3X3 f_矩阵相乘(const mat3X3& m1, const mat3X3& m2) {
	mat3X3 r;
	r.row0 = f_矩阵相乘(m1, m2.row0);
	r.row1 = f_矩阵相乘(m1, m2.row1);
	r.row2 = f_矩阵相乘(m1, m2.row2);
	return r;
}

CUDA_CALLABLE Inline mat3X3 f_矩阵相乘(const mat3X3& m1, const Matrix33& m2) {
	mat3X3 r;
	r.row0 = f_矩阵相乘(m1, *((vec3*)&m2.cols[0]));
	r.row1 = f_矩阵相乘(m1, *((vec3*)&m2.cols[1]));
	r.row2 = f_矩阵相乘(m1, *((vec3*)&m2.cols[2]));
	return r;
}







CUDA_CALLABLE std::vector<vec3> vec_Mul(const std::vector<vec3>& sv, const float32 s);
CUDA_CALLABLE std::vector<vec3> vec_Mul(const std::vector<vec4>& sv, const float32 s);




Inline vec4 f_graph_顶点构建平面(const vec3 point[3]) {
	vec3 e0 = point[1] - point[0];
	vec3 e1 = point[2] - point[0];
	vec3 n = vec_cross(e0, e1);

	float32 l = vec_dot(n, n);
	if (l > 0) {
		n *= f_逆平方根(l);
	}
	else {
		n = {};
	}
	
	return f_graph_plane(n, -vec_dot(point[0], n));
}


//====================================== 废弃 ========================================
Inline vec3 f_graph_quat变换坐标(const vec4& q, const vec3& pos) {
	vec3 newPos;
	vec3 qv = { q.x, q.y, q.z };
	newPos = pos * (2.0f * q.w * q.w - 1.0f);
	newPos = newPos - (vec_cross(qv, pos) * q.w * 2.0f) + (qv * vec_dot(vec3{ q.x, q.y, q.z }, pos) * 2.0f);
	return newPos;
}

Inline vec4 f_graph_quat旋转(const vec4& q1, const vec4& q2) {
	vec4 newQuat;
	newQuat.w = q2.w * q1.w - q2.x * q1.x - q2.y * q1.y - q2.z * q1.z;
	newQuat.x = q2.w * q1.x + q2.w * q1.x + q1.y * q2.z - q2.z * q1.y;
	newQuat.y = q2.w * q1.y - q2.x * q1.z + q2.y * q1.w + q2.z * q1.x;
	newQuat.z = q2.w * q1.z + q2.x * q1.y - q2.y * q1.x + q2.z * q1.w;
	return newQuat;
}





